//	This is the standard GCS output file for ArduPilot

/*
Message Prefixes
!!!		Position    		Low rate telemetry
+++		Attitude    		High rate telemetry
###		Mode        		Change in control mode
%%%		Waypoint    		Current and previous waypoints
XXX		Alert       		Text alert  - NOTE: Alert message generation is not localized to a function
PPP		IMU Performance		Sent every 20 seconds for performance monitoring

Message Suffix
***    All messages use this suffix
*/

/*
void acknowledge(byte id, byte check1, byte check2) {}
void send_message(byte id) {}
void send_message(byte id, long param) {}
void send_message(byte severity, const char *str) {}
*/

#if GCS_PROTOCOL == 0

void acknowledge(byte id, byte check1, byte check2)
{
}

void send_message(byte id) {
	send_message(id,0l);
}

void send_message(byte id, long param) {
	switch(id) {
		case MSG_ATTITUDE:								// ** Attitude message
			print_attitude();
			break;
		case MSG_LOCATION:								// ** Location/GPS message
			print_position();
			break;
		case MSG_HEARTBEAT:								// ** Location/GPS message
			print_control_mode();
			break;
		case MSG_PERF_REPORT:
			printPerfData();
	}
}

void send_message(byte severity, const char *str)
{

}

void print_current_waypoints(){
	print_wp(&current_loc, wp_index);
	print_wp(&next_WP, wp_index);
}

void print_control_mode(void)
{
	Serial.print("##");
	Serial.print(flight_mode_strings[control_mode]);
	Serial.println("***");
}

void print_position(void)
{
	Serial.printf_P(PSTR("!!!LAT:%ld,LON:%ld,SPD:%d,CRT:%d,ALT:%d,ALH:%d,CRS:%d,BER:%d,WPN:%d,DST:%d,***\n"),
							current_loc.lat/10,
							current_loc.lng/10,
							g_gps->ground_speed/100,
							climb_rate,
							current_loc.alt/100,
							next_WP.alt/100,
							g_gps->ground_course/100,
							target_bearing/100,
							wp_index,
							wp_distance);
}


void printPerfData(void)
{
}

void print_attitude(void)
{
	Serial.printf_P(PSTR("+++ASP:%d,THH:%d,RLL:%d,PCH:%d,***\n"),
						airspeed,
						rc_3.servo_out,
						roll_sensor/100,
						pitch_sensor/100);

}
#endif
